

    .option norvc
    .section .text.init,"ax",@progbits
  
    .globl bl2_entrypoint
bl2_entrypoint:
    
    // atf_state_set x28, x29, ATF_STATE_BL2_ENTRY_POINT
    // debug trace ???? 
    addiw   t3, zero, 89
    slli    t3, t3, 0x19        # 0xB2000000
    lui     t4, 0x3000
    sw      t3, 132(t4)         # 3000084

    
    ### write mtvec and make sure it sticks
    la      t0, trap_vector
    csrw    mtvec, t0

    ### set mxstatus to init value
    li      t1, 0xc0638000
    csrw    mxstatus, t1

    ### set plic_ctrl = 1,  plic_base = mapbaddr = 0x7000-0000
    li      t0, 0x701FFFFC        # plic_base + 0x1FFFFC
    li      x4, 1
    sw      x4, 0(t0)

    ### invalid I-cache
    li      t0, 0x33
    csrc    mcor, t0
    li      t0, 0x11
    csrs    mcor, t0

    ### enable I-cache
    li      t0, 0x1
    csrs    mhcr, t0

    ### invalid D-cache
    li      t0, 0x33
    csrc    mcor, t0
    li      t0, 0x12
    csrs    mcor, t0

    ### enable D-cache
    li      t0, 0x2
    csrs    mhcr, t0

    ### zero bss section
    la      a3, __BSS_START__
    la      a4, __BSS_END__
    sub     a4, a4, a3
    beqz    a4, all_ready

bss_clear:
    sd      zero, 0(a3)
    addi    a3, a3, 8
    addi    a4, a4, -8
    bnez    a4, bss_clear

all_ready:
    
    ### excp/ints stack : 0x8400_0000 
    lui     sp, 0x84000
    csrw    mscratch, sp

    ### main loop stack : 0x83f0_0000 
    lui     sp, 0x83f00

    // call to C function.
    call    SystemInit
    call    main
    call    sys_reset

die_loop:
    nop
    j       die_loop

    

    .balign 4
trap_vector:
    
    // switch stack pointer
    
    // 如果不考虑中断处理中触发异常, 导致出现 2 曾嵌套, 
    // 可以直接使用 sp , 而不用通过减法来获取空间, 可以减小中断响应延迟.

    // todo : 考虑 MTVEC 中的 向量模式, 可以让 异常和中断 分开处理.
    csrrw   sp, mscratch, sp
    addi    sp, sp, -256

    // save regs...
    sd      ra, 0(sp)       // x1
    sd      gp, 8(sp)       // x3
    sd      tp, 16(sp)      // x4

    sd      a0, 32(sp)      // x10, x11, x12, x13, x14, x15, x16, x17
    sd      a1, 40(sp)      
    sd      a2, 48(sp)
    sd      a3, 56(sp)
    sd      a4, 64(sp)
    sd      a5, 72(sp)
    sd      a6, 80(sp)
    sd      a7, 88(sp)
    
    sd      t0, 96(sp)      // x5, x6, x7, x28, x29, x30, x31
    sd      t1, 104(sp)      
    sd      t2, 112(sp)      
    sd      t3, 120(sp)
    sd      t4, 128(sp)
    sd      t5, 136(sp)
    sd      t6, 144(sp)

    // 判断最高 bit 是否为 1, 
    csrr    a0, mcause
    blt     a0, zero, intproc
    
    mv      a1, sp
    call    trap_handler
    jal     zero, ctx_pops

intproc:
    andi    a0, a0, 0x1f
    call    intr_handler

ctx_pops:
    // restore regs...
    ld      ra, 0(sp)       // x1
    ld      gp, 8(sp)       // x3
    ld      tp, 16(sp)      // x4
    
    ld      a0, 32(sp)      // x10, x11, x12, x13, x14, x15, x16, x17
    ld      a1, 40(sp)
    ld      a2, 48(sp)
    ld      a3, 56(sp)
    ld      a4, 64(sp)
    ld      a5, 72(sp)
    ld      a6, 80(sp)
    ld      a7, 88(sp)
    
    ld      t0, 96(sp)      // x5, x6, x7, x28, x29, x30, x31
    ld      t1, 104(sp)
    ld      t2, 112(sp)
    ld      t3, 120(sp)
    ld      t4, 128(sp)
    ld      t5, 136(sp)
    ld      t6, 144(sp)

    addi    sp, sp, 256
    csrrw    sp, mscratch, sp
    mret


    
    

